#include "ros/ros.h"
#include "serial/serial.h"  //ROS已经内置了的串口包 
#include "std_msgs/String.h"
#include "std_msgs/Empty.h" 
#include "sensor_msgs/NavSatFix.h"
#include "test/checksum.h"
#include "test/serial.h"
#include "test/serialProtocol.h"
#include "fstream"
#include <stdio.h>
#include <iostream>

extern RingFifo_t gGPS_msg_buffer;
extern RingFifo_t guwb_msg_buffer;



#define length 32
#define measure_length 6
std::ofstream GPS_file;
std::ofstream UWB_file;
std::ofstream P900_file;

char GPS_file_name[100];
char UWB_file_name[100];
char P900_file_name[100];

serial::Serial ser_900; //声明串口对象 
serial::Serial ser_UWB; //声明串口对象
  
//回调函数 
void write_callback(const sensor_msgs::NavSatFix::ConstPtr& msg) 
{ 
    ROS_INFO("Get self_msg");
    uint64_t timestamp = ros::Time::now().toNSec();
    uint32_t timestamp_u=(timestamp/1000)&0xffffffff;
    if(!GPS_file.is_open())
    {
        GPS_file.open (GPS_file_name,std::ofstream::app);
    }
    
    GPS_file<<timestamp_u<<" "<<msg->latitude<<" "<<msg->longitude<<" "<<msg->altitude;
    GPS_file<<"\n";
} 
  
int main (int argc, char** argv) 
{ 
    //初始化节点 
    ros::init(argc, argv, "serial_example_node"); 
    ROS_INFO("1");
    //声明节点句柄 
    ros::NodeHandle nh; 
    ROS_INFO("2");
    //订阅主题，并配置回调函数 
    ros::Subscriber write_sub = nh.subscribe("/mavros/global_position/global", 1000, write_callback); 
    //发布主题 
    ROS_INFO("3");
    double now_time=ros::Time::now().toSec();
    sprintf(GPS_file_name,"/home/ros-18/node_test_chong/GPS_store_%f.txt",now_time);
    sprintf(UWB_file_name,"/home/ros-18/node_test_chong/UWB_store_%f.txt",now_time);
    sprintf(P900_file_name,"/home/ros-18/node_test_chong/P900_store_%f.txt",now_time);
    ROS_INFO("4");
    

    try 
    { 
    //设置串口属性，并打开串口 
        ser_900.setPort("/dev/ttyUSB0"); 
        ser_900.setBaudrate(115200); 
        serial::Timeout to1 = serial::Timeout::simpleTimeout(1000); 
        ser_900.setTimeout(to1); 
        ser_900.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("P900_error:Unable to open port_p900 "); 
        return -1; 
    } 
  
    //检测串口是否已经打开，并给出提示信息 
    if(ser_900.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port_p900 initialized"); 
    } 
    else 
    { 
        return -1; 
    }
    //开启UWB的串口
    try 
    { 
    //设置串口属性，并打开串口 
        ser_UWB.setPort("/dev/ttyUSB1"); 
        ser_UWB.setBaudrate(230400); 
        serial::Timeout to2 = serial::Timeout::simpleTimeout(1000); 
        ser_UWB.setTimeout(to2); 
        ser_UWB.open(); 
    } 
    catch (serial::IOException& e) 
    { 
        ROS_ERROR_STREAM("Unable to open port_UWB  "); 
        return -1; 
    } 
  
    //检测串口是否已经打开，并给出提示信息 
    if(ser_UWB.isOpen()) 
    { 
        ROS_INFO_STREAM("Serial Port_UWB initialized"); 
    } 
    else 
    { 
        return -1; 
    }
  
    //指定循环的频率 
    ros::Rate loop_rate(25);
    size_t count_900=0;
    size_t count_UWB=0; 
    uint8_t tempData=0;
    int i ;

    while(ros::ok()) 
    { 
        count_900=ser_900.available();
        //printf("count_900=%d\n",count_900);
        count_UWB=ser_UWB.available();
        //printf("count_UWB=%d\n",count_900);
        if(count_900){ 
            ROS_INFO("main_P900:Reading from serial port"); 
            uint8_t* buffer = new uint8_t[count_900];
            size_t count=ser_900.read(buffer,count_900); 
           
            if(count==(count_900))
            {
                ROS_DEBUG("main_P900:read success!");
            }
            else
            {
                ROS_DEBUG("main_P900:read failed!");
            }
            if(!P900_file.is_open())
            {
                 P900_file.open (P900_file_name,std::ofstream::app);//std::ofstream::binary|
                 
            }
            if (RINGFIFO_COUNT(gGPS_msg_buffer) + count <= RINGFIFO_SIZE){
                for(int i=0 ;i < count ;i++ ){
                    RINGFIFO_WR(gGPS_msg_buffer,buffer[i]);
                }
            }else{
                ROS_DEBUG("Buffer overflow");
            }
            uint64_t P900_time=ros::Time::now().toNSec();
            uint32_t P900_time_m=(P900_time/1000)&0xffffffff;

            uint8_t gpsInfoMsgBuffer[GPS_MSG_LEN];
            gps_info gpsinfo ;

            if( RINGFIFO_EMPTY(gGPS_msg_buffer) == false)
            {
                while(RINGFIFO_COUNT(gGPS_msg_buffer)>=GPS_MSG_LEN){
                    RINGFIFO_RD(gGPS_msg_buffer,tempData);
                    if(tempData == MSG_HEADER)
                    {
                        gpsInfoMsgBuffer[0]= tempData;
                        for(i=1;i<GPS_MSG_LEN;i++){
                            RINGFIFO_RD(gGPS_msg_buffer,tempData);
                            gpsInfoMsgBuffer[i] = tempData;
                        }
                        if(msg_gps_unpack(&gpsinfo,gpsInfoMsgBuffer)==UNPACK_SUCCESS){
                             P900_file<<P900_time_m<<" "<<gpsinfo.timestamp<<" "<<gpsinfo.lon<<" "<<gpsinfo.lat<<" "<<gpsinfo.height<<" "<<gpsinfo.vel_n<<" "<<" "<<gpsinfo.vel_e<<" "<<gpsinfo.vel_d;
                             P900_file<<"\n";    
                        }
                        else{
                            ROS_DEBUG("unpack unsuccessful");
                        }
                    }
                }
            }
            
            delete[] buffer;

           // P900_file.close();
        } 
        if(count_UWB){ 
            ROS_INFO("main_UWB:Reading from serial port"); 
            uint8_t* buffer = new uint8_t[count_UWB];
            size_t count=ser_UWB.read(buffer,count_UWB); 
            if(count==count_UWB)
            {
                ROS_INFO("main_UWB:read success!");
            }
            else
            {
                ROS_DEBUG("main_UWB:read failed!");
            }
            if(!UWB_file.is_open())
            {
                 UWB_file.open (UWB_file_name,std::ofstream::app);//std::ofstream::binary|
            }
            if (RINGFIFO_COUNT(guwb_msg_buffer) + count <= RINGFIFO_SIZE){
                for(int i=0 ;i < count ;i++ ){
                    RINGFIFO_WR(guwb_msg_buffer,buffer[i]);
                }
            }else{
                ROS_DEBUG("Buffer overflow");
            }
            uint64_t UWB_time=ros::Time::now().toNSec();
            uint32_t UWB_time_m=(UWB_time/1000)&0xffffffff;

            uint8_t uwbInfoMsgBuffer[GPS_RANGE_MSG_LEN];
            gps_range_info uwbinfo ;

            if( RINGFIFO_EMPTY(guwb_msg_buffer) == false)
            {
                while(RINGFIFO_COUNT(guwb_msg_buffer)>=GPS_RANGE_MSG_LEN){
                    RINGFIFO_RD(guwb_msg_buffer,tempData);
                    if(tempData == MSG_HEADER)
                    {
                        uwbInfoMsgBuffer[0]= tempData;
                        for(i=1;i<GPS_RANGE_MSG_LEN;i++){
                            RINGFIFO_RD(guwb_msg_buffer,tempData);
                            uwbInfoMsgBuffer[i] = tempData;
                        }
                        if( msg_gps_range_unpack(&uwbinfo,uwbInfoMsgBuffer)==UNPACK_SUCCESS){
                            UWB_file<<UWB_time_m<<" "<<uwbinfo.range_anchor1<<" "<<uwbinfo.range_anchor2<<" "<<uwbinfo.range_anchor3;
                            UWB_file<<"\n";    
                        }
                        else{
                            ROS_DEBUG("unpack unsuccessful");
                        }
                    }
                }
            }
            
            // release dynamically-allocated memory
            delete[] buffer;

           // UWB_file.close();
        }
        loop_rate.sleep();
    }

        //处理ROS的信息，比如订阅消息,并调用回调函数 
        //ros::spin(); 
       // loop_rate.sleep(); 
  
    //} 
} 
